#include "dog.hpp"
#include "evtinfo.hpp"

Dog::Dog(int debug)
{
    int cnt = 0;

    /* 调试模式标记位 */
    this->debug = debug;

    /* Go2 前置摄像头推流 */
    // cmdline = "rtmp://vlog16.aiit.org.cn/live/dog";
    cmdline = "rtmp://115.236.53.230/live/dog";
    if (debug) {
        cmdline = "rtmp://10.0.106.15/live/dog";
    }
    cam.set(cmdline);
    cam.start();

    /* 位置上报+事件接收 */
    pos.start();

    /* 雷达定位 */
    nav.pos_thread_start();
    nav.nav_start();

    /* 手动巡逻 */
    // pat.start();
}

Dog::~Dog()
{
    pos.stop();
    cam.stop();
}

void Dog::start()
{
    if (!handle.joinable()) {
        running = true;
        handle = std::thread(&Dog::thread, this);
    }
}

void Dog::stop()
{
    if (handle.joinable()) {
        running = false;
        handle.join();
    }
}

void Dog::thread()
{
    bool valid; /* 数据有效位 */
    int type = -1; /* 事件类型 */
    float curr_x = 0.0f; /* 当前x坐标 */
    float curr_y = 0.0f; /* 当前y坐标 */
    float curr_a = 0.0f; /* 当前a坐标 */

#if 0
    /* 派出所事件点位 2.57448, 6.22701, 1.60413 */
    float dest_x = 2.57448; /* 目标x坐标 */
    float dest_y = 6.22701; /* 目标y坐标 */
    float dest_a = 1.60413; /* 目标a坐标 */

    float pause_x = 0.0f; /* 暂停点x坐标 */
    float pause_y = 0.0f; /* 暂停点y坐标 */
    float pause_a = 0.0f; /* 暂停点a坐标 */
#endif

#if 1
    /* 智慧谷展厅点位 */
    float dest_x = 2.57448; /* 目标x坐标 */
    float dest_y = 6.22701; /* 目标y坐标 */
    float dest_a = 1.60413; /* 目标a坐标 */

    float pause_x = -3.0f; /* 暂停点x坐标 */
    float pause_y = 0.48f; /* 暂停点y坐标 */
    float pause_a = 0.0f; /* 暂停点a坐标 */
#endif

    while (running) {
        /* 检查是否有事件需要处理 */
        EvtInfo::get(valid, type);
        if (!valid) {
            msleep(100);
            continue;
        }
        EvtInfo::set(false, EVT_TYPE_NONE);

        if (type == EVT_TYPE_MOVE) {
            std::cout << "Dog: move event" << std::endl;

            /* 判断有没有到A坐标附近 */
            while (1) {
                /* 获取当前位置信息 */
                PosInfo::get(valid, curr_x, curr_y, curr_a);
                if (valid == false) {
                    std::cout << "Dog: get position failed!" << std::endl;
                    msleep(500);
                    continue;
                }

                /* 计算目标位置和当前位置距离和偏差角度 */
                float dx = pause_x - curr_x;
                float dy = pause_y - curr_y;
                float distance = std::sqrt(dx * dx + dy * dy);
                std::cout << "Dog move event: x = " << curr_x << ", y = " << curr_y << ", dist = " << distance << std::endl;

                /* 到达目标位置附近，则退出 */
                if (distance < 0.2) {
                    std::cout << "Dog move event done, break" << distance << std::endl;
                    break;
                }
                msleep(500);
            }

            /* 暂停导航，记录当前坐标(其实就是之前的A坐标) */
            nav.nav_pause();
            std::cout << "Dog: move event pause" << std::endl;

#if 0
            /* 运动到B坐标位置 */
            motion.sport_position(dest_x, dest_y, dest_a);
#endif

            /* 停留在B坐标位置 */
            // motion.stopMove();

            /* 回复网页已经运动到位 */
            pos.set_send_flag();

        } else if (type == EVT_TYPE_RECOVER) {
            std::cout << "Dog: recover event" << std::endl;

#if 0
            /* 运动到暂停点 */
            motion.sport_position(pause_x, pause_y, pause_a);
#endif

            /* 恢复导航 */
            nav.nav_recover();
            std::cout << "Dog: move event recover" << std::endl;

        } else {
            std::cout << "Dog: unknown event" << std::endl;
        }
    }
}

void Dog::msleep(int ms)
{
    usleep(ms * 1000);
}
